#include <WProgram.h>
#include "IAcomm.h"

//#define DEBUG
#include "debug.h"

uint32_t IAcomm::SYNC_WAIT_TIME = SYNC_WAIT_TIME_DEF;
uint32_t IAcomm::CMD_RESPONSE_WAIT_TIME = CMD_RESPONSE_WAIT_TIME_DEF;
static const uint8_t MAX_SERIAL_BUFSIZE = 64;

IAcomm::IAcomm(uint8_t rxPin, uint8_t txPin, uint16_t spd, IAhandler handler): 
  methodId(0), cmdSent(0), cmdRcvd(0), numArgBytes(0), argCRC(0),
  peerMethodId(0),peerCmdRcvd(0),peerDataBytes(0),peerCRC(0),
  commStatus(NONE),rcvHandler(handler),speed(spd), peer(rxPin,txPin)
{
  peer.begin(speed);
}

// Called when data received from peer.
// Side effect: increments receive counter.
bool IAcomm::buf2fields() 
{
  bool ret = true;
  peerMethodId = hdrBuf[1];
  cmdRcvd = hdrBuf[2];
  peerCmdRcvd = hdrBuf[3];
  peerDataBytes = (hdrBuf[4] << 8);
  peerDataBytes += hdrBuf[5];
  peerCRC = (hdrBuf[6] << 8);
  peerCRC += hdrBuf[7];
  
  _DPRINT_PEER_HDR();

  // do checks.
  _DPRINT3("cmdRcvd",cmdRcvd,HEX);
  if (hdrBuf[0] != 0xAA) {
    _DPRINT3("magic",hdrBuf[0],HEX);
    ret = false;
  }
  if (peerDataBytes > MAX_COMM_BUF_SIZE) {
    _DPRINT3("peerDataBytes",peerDataBytes,HEX);
    ret = false;
  }

  return ret;
}



// Called to send data to peer.
// Side effect: increments send counter.
void IAcomm::fields2buf()
{
  hdrBuf[0] = 0xAA;
  hdrBuf[1] = methodId;
  hdrBuf[2] = ++cmdSent;
  hdrBuf[3] = cmdRcvd;
  hdrBuf[4] = (numArgBytes >> 8);
  hdrBuf[5] = (numArgBytes & 0xFF);
  hdrBuf[6] = (argCRC >> 8);
  hdrBuf[7] = (argCRC & 0xFF);

  _DPRINT_HDR();
}

void IAcomm::setData(uint8_t method, uint16_t len, uint8_t* data, bool syncResponse)
{
  methodId = method;
  numArgBytes = len;
  dataBuf = data;
  if (syncResponse) {
    argCRC = 1;
  } else {
    CRC();
  }
  fields2buf();  
}


void IAcomm::writeHdr2Net()
{
  peer.Print::write((uint8_t*)&hdrBuf,sizeof(hdrBuf));
}

bool IAcomm::writeData2Net(int32_t deadline)
{
  if (numArgBytes) {
    peer.Print::write((uint8_t*)dataBuf,numArgBytes);
  }
  return waitForSyncResponse(deadline);
}

bool IAcomm::send(uint8_t method, uint8_t* data, uint16_t length,
		  int32_t waitMs /* =CMD_RESPONSE_WAIT_TIME_DEF */)
{
  if (commStatus != NONE) {
    _DPRINT("bad entry");
    return false;
  }

  if (!method) {		// SYNC = 0, is private.
    _DPRINT("bad method.");
    return false;
  }
  commStatus = WRITING;
  bool ret = false;

  int32_t deadline = waitMs;
  if (waitMs > 0) {
    deadline += millis();
  }

  setData(method,length,data);
  writeHdr2Net();
  if (writeData2Net(deadline)) {
    ret = true;
  }

  commStatus = NONE;
  return ret;
}

void IAcomm::serialCheck()
{
  if (peer.overflow()) {
    _DPRINT("Serial:Overflow!");
  }
}

// Reads the header off the network and acts on the
// message. If sync response needed, sends one. If data
// to be read, handles it.
bool IAcomm::readPeer()
{
  if (!peer.available() || !readHdr()) return false;

  if (methodIsSync()) {
    // get to the last sync request if available.
    while (peer.available() && readHdr() && methodIsSync());
    if (methodIsSync()) {
      sendSyncResponse();
      return true;
    }
    // else fall thru' to handle last hdr received.
  }

  if (userMethod()) {
    if (!recvData()) return false;
    sendSyncResponse();
    (*rcvHandler)(peerMethodId, peerDataBuf, peerDataBytes);
    return true;
  }

  // Else could be a sync Response.
  return true;
}

bool IAcomm::poll(uint32_t waitMs /* = POLL_WAIT_TIME_DEF */)
{
  if (commStatus != NONE) {
    _DPRINT("bad entry");
    return false;
  }
  commStatus = READING;
  bool ret = false;
  if (! waitMs) {
    ret = readPeer();
  } else {
    uint32_t deadline = millis() + waitMs;
    while (millis() < deadline) {
      if (readPeer()) ret = true;
    }
  }
  
  commStatus = NONE;
  return ret;
}

bool IAcomm::readHdr()
{
  uint8_t available;
  uint8_t needed = sizeof(hdrBuf);
  uint8_t* bufP = hdrBuf;

  for (uint8_t rep=3; rep; rep--) { // to account for link delays.

    while (!(available=peer.available()) && available<needed);
    if (needed<=available) {
      while (needed--) *bufP++ = peer.read();
      if (!buf2fields()) return false;
      return true;
    }
    delay(10);

  } 
  _DPRINT3("available",available,HEX);
  return false;
}

bool IAcomm::recvData()
{
  return (!peerDataBytes || (readData() && checkCRC()));
}

// Only reason to have a timeout is so we don't
// wait indefinitely in case of network errors.
bool IAcomm::readData()
{
  if (peerDataBytes > MAX_COMM_BUF_SIZE) {
    _DPRINT("data len");
    return false;
  }

  bool gotIt=false;
  uint32_t deadline = millis() + CMD_RESPONSE_WAIT_TIME_DEF;
  while (millis() < deadline) {
    if (peer.available()>=peerDataBytes) {
      gotIt = true;
      break;
    }
  }
  if (!gotIt) {
    _DPRINT4("data",peerDataBytes,peer.available(),HEX);
    return false;
  }
  uint8_t* bufP = peerDataBuf;
  for (uint16_t n=peerDataBytes; n; n--) {
       *bufP++ = peer.read();
  }
  return true;
}  


bool IAcomm::sync(int32_t waitMs /* =SYNCWAIT_TIME_DEF */)
{
  if (commStatus != NONE) {
    _DPRINT("bad entry");
    return false;
  }
  commStatus = SYNC;

  uint32_t cmdDeadline;
  bool ret = false;
  if (!waitMs) {
    sendSync();
    ret = waitForSyncResponse(waitMs);

  } else {

    uint32_t syncDeadline = millis() + waitMs;
    while (waitMs<0? true: millis() < syncDeadline) {
      sendSync();
      cmdDeadline = millis() + CMD_RESPONSE_WAIT_TIME;
      if (waitForSyncResponse(cmdDeadline)) {
	ret = true;
	break;
      }
    }
  }

  commStatus = NONE;
  return ret;
}

bool IAcomm::waitForSyncResponse(int32_t deadline)
{
  uint8_t cmd = cmdSent;

  if (!deadline) {
    if (readPeer() && methodIsSyncResponse(cmd)) {
	return true;
    }
    // else fall thru'

  } else if (deadline < 0) {
    while (!readPeer() || !methodIsSyncResponse(cmd));
    return true;

  } else {
    while ( millis() < deadline ) {
      if (readPeer() && methodIsSyncResponse(cmd)) {
	return true;
      }
    }
    // else fall thru'
  }
  // we only get here if timed out.
  _DPRINT3("response! cmdSent:",cmd,HEX);
  return false;
}

void IAcomm::sendSync()
{
  setData(M_SYNC,0,0);
  writeHdr2Net();
}

// Send syncResponse if needed. ie. if no data or SYNC message.
void IAcomm::sendSyncResponse()
{
  setData(M_SYNC,peerDataBytes,0,true);
  writeHdr2Net();
}

void IAcomm::CRC()
{
  argCRC = 0;
  for (uint16_t i=0;i<numArgBytes; i++) argCRC += dataBuf[i];
}

bool IAcomm::checkCRC()
{
  uint16_t crc = 0;
  for (uint16_t i=0;i<peerDataBytes; i++) crc += peerDataBuf[i];  
  if (crc == peerCRC) return true;

  _DPRINT3("cmdRcvd",cmdRcvd,HEX);
  _DPRINT3("my crc",crc,HEX);
  _DPRINT3("peerCRC",peerCRC,HEX);
}
